#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <thread>
#include "serial_kalman.h"
#include "kalmanfilter.h"
#include <chrono>
#include <iomanip>
#include <modbus/modbus.h>

const int RETRY_DELAY_MS = 1000; // Delay between connection retries

// Convert various data types to 8-bit byte vector
template <typename T>
std::vector<uint8_t> to_byte_vector(const T &value)
{
    std::vector<uint8_t> bytes(sizeof(T));
    std::memcpy(bytes.data(), &value, sizeof(T));
    return bytes;
}

bool connect_modbus(modbus_t *&mb)
{
    while (true)
    {
        if (modbus_connect(mb) == -1)
        {
            std::cerr << "Connection failed: " << modbus_strerror(errno) << ". Retrying..." << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
        }
        else
        {
            std::cout << "Connected successfully!" << std::endl;
            if (modbus_set_slave(mb, 1) == -1)
            {
                std::cerr << "Modbus set slave failed: " << modbus_strerror(errno) << std::endl;
                modbus_free(mb);
                return false;
            }
            return true;
        }
    }
}

int main(int argc, char *argv[])
{
    if (argc != 3)
    {
        std::cerr << "Usage: " << argv[0] << " <server_ip>" << " <sample_interval>" << std::endl;
        return 1;
    }

    std::string server_ip = argv[1];
    int sample_interval = std::stoi(argv[2]);

    std::cout << " +----------------------------------+" << std::endl;
    std::cout << " |         MMW Measurement          |" << std::endl;
    std::cout << " +----------------------------------+" << std::endl;

    int mode;
    std::cout << "Select Mode (1 only measure; 2 measure and send): ";
    std::cin >> mode;

    // 卡尔曼滤波参数初始化
    Eigen::VectorXd x(3);
    x << 345, 0, 0;
    // 这里因为测量时间有波动，所以通过函数，动态传入
    // Eigen::MatrixXd F(3, 3);
    // F << 1, 2, 2,
    //      0, 1, 2,
    //      0, 0, 1;
    Eigen::MatrixXd p = Eigen::MatrixXd::Zero(3, 3);
    p(0, 0) = 10;
    p(1, 1) = 10;
    p(2, 2) = 10;
    // 调参经验：Q中的位置噪声与R一起来看，如果位置噪声与观测噪声一样，则滤波结果就在预测结果和观测结果中间
    //         Q中的速度噪声越大，则画出来的滤波曲线越不平滑，因为速度没有观测值，所以速度噪声越大，预测结果和滤波结果就会越向计算出来的速度值靠近，也就陡峭
    //         Q中的加速度噪声越大，则画出来的滤波曲线越不平滑，并且加速度噪声会略微影响速度的滤波结果
    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(3, 3);
    Q(0, 0) = 0.1;
    Q(1, 1) = 10;
    Q(2, 2) = 0.01;
    // 扩展到两个距离传感器，就写成
    // R = [0.1 0 ;
    //     0 0.1];
    // 具体值看传感器精度
    // 三个传感器同理
    Eigen::MatrixXd R = Eigen::MatrixXd::Zero(1, 1); // 三个传感器的噪声协方差矩阵
    R(0, 0) = 0.1;
    // R(1, 1) = 0.15;
    // R(2, 2) = 0.15;
    // 扩展到两个距离传感器，就写成
    // H = [1 0 0;
    //     1 0 0];
    // 三个传感器同理
    Eigen::MatrixXd H(1, 3);
    H << 1, 0, 0; // 扩展到三个传感器
    KalmanFilter kalmanfilter(x, p, Q, R, H);

    // 初始化串口对象。并通过向雷达发送write_buffer以获取雷达读数
    std::vector<uint8_t> write_buffer = {0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A};
    SerialWrapper serial_wrapper("/dev/ttyUSB0", 115200, boost::asio::serial_port_base::character_size(8),
                                 boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none),
                                 boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one), sample_interval,
                                 kalmanfilter);
    // 查看是否存在 data_client.csv 文件，如果不存在就创建，如果存在就不做什么。如果注销此行，就不会存再将数据保存到文件
    serial_wrapper.ensure_file_exists("data_client.csv", true);

    // 两个模式，模式1是只测量，模式2是测量并发送数据
    if (serial_wrapper.open_serial())
    {
        if (mode == 1)
        {
            auto empty_callback = [](uint64_t, uint16_t, double, uint16_t) {};
            auto empty_kalman_callback = [](uint64_t, uint16_t, double, uint16_t, double, double) {};
            std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
            std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
            std::thread speed_thread(&SerialWrapper::kalman_estimate_speed, &serial_wrapper, empty_kalman_callback);

            send_thread.join();
            receive_thread.join();
            speed_thread.join();
        }
        else if (mode == 2)
        {
            modbus_t *mb = modbus_new_tcp(server_ip.c_str(), MODBUS_TCP_DEFAULT_PORT);
            if (!mb || !connect_modbus(mb))
            {
                return -1;
            }

            // 设置Modbus从站地址
            modbus_set_slave(mb, 1); // 替换为你的从站地址
            
            uint16_t tab_reg[32];
            // 没有卡尔曼滤波时使用这个回调函数
            auto speed_callback = [&mb, &tab_reg](uint64_t timestamp, uint16_t current_measurement, double real_speed, uint16_t dir)
            {
                tab_reg[0] = timestamp >> 48;
                tab_reg[1] = timestamp >> 32;
                tab_reg[2] = timestamp >> 16;
                tab_reg[3] = timestamp;
                tab_reg[4] = current_measurement;
                std::memcpy(&tab_reg[5], &real_speed, sizeof(real_speed));
                tab_reg[9] = dir;

                if (modbus_write_registers(mb, 40102, 10, tab_reg) == -1)
                {
                    std::cerr << "Failed to write to register: " << modbus_strerror(errno) << std::endl;
                    modbus_close(mb);
                    connect_modbus(mb); // Retry connection on failure
                }
                else
                {
                    std::cout << "Data sent to Modbus server" << std::endl;
                }
            };
            // 有卡尔曼滤波时使用这个回调函数
            auto kalman_speed_callback = [&mb, &tab_reg](uint64_t timestamp, uint16_t current_measurement, double real_speed, uint16_t dir, double kalman_dis, double kalman_vel)
            {
                tab_reg[0] = timestamp >> 48;
                tab_reg[1] = timestamp >> 32;
                tab_reg[2] = timestamp >> 16;
                tab_reg[3] = timestamp;
                tab_reg[4] = current_measurement;
                std::memcpy(&tab_reg[5], &real_speed, sizeof(real_speed));
                tab_reg[9] = dir;
                std::memcpy(&tab_reg[10], &kalman_dis, sizeof(kalman_dis));
                std::memcpy(&tab_reg[14], &kalman_vel, sizeof(kalman_vel));

                if (modbus_write_registers(mb, 0, 18, tab_reg) == -1)
                {
                    std::cerr << "Failed to write to register: " << modbus_strerror(errno) << std::endl;
                    modbus_close(mb);
                    connect_modbus(mb); // Retry connection on failure
                }
                else
                {
                    std::cout << "Data sent to Modbus server" << std::endl;
                }
            };

            std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
            std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
            std::thread speed_thread(&SerialWrapper::kalman_estimate_speed, &serial_wrapper, kalman_speed_callback);

            send_thread.join();
            receive_thread.join();
            speed_thread.join();

            modbus_close(mb);
            modbus_free(mb);
        }
        else
        {
            std::cerr << "Invalid mode selected." << std::endl;
        }
    }

    serial_wrapper.close();
    std::cout << "\nSerial communication ended" << std::endl;

    return 0;
}
